// SEE END OF FILE FOR LICENSE TERMS

#include "mixer.h"
#include "../libCAN/libCAN.h"
#include "../CANInterface/CANInterface_defines.h"

// The array where the autopilot commands are put before mixing.
float APOutputCommands[APCON_MAX_COMMANDS];

// A count of the number of mixers used.
// IT IS VERY IMPORTANT TO SET THIS CORRECTLY. FAILURE MEANS UNKNOWN BEHAVOIR.
const unsigned int MixerCount = 12;

// Mixer settings
const MixerSettings Mixers[MAX_MIXERS] = {
		1,1,	RUDDER_INPUT_CHANNEL,	RUDDER_OUTPUT_CHANNEL	,1,1,0,
		1,1,	ELEVATOR_INPUT_CHANNEL,	ELEVATOR_OUTPUT_CHANNEL	,1,1,0,
		1,1,	THROTTLE_INPUT_CHANNEL,	THROTTLE_OUTPUT_CHANNEL	,1,1,0,
		1,1,	AILERON_INPUT_CHANNEL,	AILERON_OUTPUT_CHANNEL	,1,1,0,
		1,1,	AILERON2_INPUT_CHANNEL,	AILERON_SECONDARY_OUTPUT_CHANNEL ,1,1,0,
		1,1,	FLAP1_INPUT_CHANNEL,	FLAP1_OUTPUT_CHANNEL	,1,1,0,
		1,1,	FLAP2_INPUT_CHANNEL,	FLAP2_OUTPUT_CHANNEL	,1,1,0,

		1,1,	APCON_THROTTLE,		THROTTLE_OUTPUT_CHANNEL		,0,0,0,
		-1.0,-1.0,APCON_YAW,		RUDDER_OUTPUT_CHANNEL		,0,0,0,
		0.5,0.5,APCON_PITCH,		ELEVATOR_OUTPUT_CHANNEL		,0,0,0,
		0.5,1.0,APCON_ROLL,			AILERON_OUTPUT_CHANNEL		,0,0,0,
		1.0,0.5,APCON_ROLL,			AILERON_SECONDARY_OUTPUT_CHANNEL ,0,0,0,	};


void CAN_received_control_outputs_callback()
{
};

void servoMix()
{
	int MixerNo = sizeof(Mixers);
	int Destination;	// Index of destination channel
	int Source;
	MixerSettings* pMixSettings = &Mixers;

	float SourceVal;
	float DestVal;
	float MixVal;
	float Coefficient;

	int DestTemp[MAX_OUTPUTS];	// Will put sum of mixer in here.

// Setup all destinations to trim points for safety
	for(Destination = 0; Destination < MAX_OUTPUTS; Destination++)
	{
		DestTemp[Destination] = udb_pwTrim[Destination];
	};

	APOutputCommands[APCON_ROLL] 		= (float) roll_control;
	APOutputCommands[APCON_PITCH] 		= (float) pitch_control;
	APOutputCommands[APCON_YAW] 		= (float) yaw_control;
	APOutputCommands[APCON_THROTTLE] 	= (float) altitude_control;
	APOutputCommands[APCON_ALTITUDE] 	= (float) altitude_control;
	APOutputCommands[APCON_BRAKES] 		= 0;
	APOutputCommands[APCON_FLAPS]		= 0;
	APOutputCommands[APCON_CAMBER] 		= 0;

// Step through each mixer.  Find source and destination for each mixer
	for(MixerNo = 0; MixerNo < MixerCount; MixerNo++)
	{
		pMixSettings = &(Mixers[MixerNo]);

		Source 		= pMixSettings->Source;
		Destination = pMixSettings->Destination;

		// Source value is always offset from zero.
		if(pMixSettings->SourceIsInput == 1)
		{
			SourceVal = udb_pwIn[Source] - udb_pwTrim[Source];	// Remove zero trim point for inputs.

			// This controls the waggles. Only waggle inputs, not autopilot commands.
			if( (Source == APCON_ROLL) ||
				(Source == APCON_PITCH) )
			{
				SourceVal += waggle;
			};
		}
		else
		{
			SourceVal = APOutputCommands[Source];
		};

		// Pick the positive or negative coefficient.
		if(SourceVal >= 0)
			Coefficient = pMixSettings->PosCoeff;
		else
			Coefficient = pMixSettings->NegCoeff;

		// Mix
		MixVal = Coefficient * SourceVal;

		// If forcing the value instead of mixing, add the trim offset back to the mix.
		if(pMixSettings->ForceValue == 1)
			DestTemp[Destination] = MixVal + udb_pwTrim[Source];
		else
			DestTemp[Destination] += MixVal;
	};

// Loop through temporary outputs and adjust for servo saturation.
	for(Destination = 0; Destination < MAX_OUTPUTS; Destination++)
	{
		DestTemp[Destination] = udb_servo_pulsesat(DestTemp[Destination]);
	};

	memcpy(&udb_pwOut, &DestTemp, sizeof(udb_pwOut));
};
